#include "rrquadric.h"
#include <cmath>

RRQuadric::RRQuadric()
{
    type = RR_QUADRIC;
}

void RRQuadric::rrTrace(RRRay *ray, RRCrossInfo *arg)
{
    double D;
    double AA, BB, CC;
    RRVec3 d = ray->rrGetDir();
    RRVec3 r = ray->rrGetSrc();
    double dz = degree_z;
    A = 1.0 / (size.x * fabs(size.x));
    B = 1.0 / (size.y * fabs(size.y));
    C = 1.0 / (size.z * fabs(size.z));
    double Xd, Yd, Zd, Xc, Yc, Zc;
    Xd = cos(dz) * d.x - sin(dz) * d.y;
    Xc = cos(dz) * r.x - sin(dz) * r.y - coord.x;
    Yd = sin(dz) * d.x + cos(dz) * d.y;
    Yc = sin(dz) * r.x + cos(dz) * r.y - coord.y;
    Zd = d.z;
    Zc = r.z - coord.z;
    AA = A * Xd * Xd + B * Yd * Yd + C * Zd * Zd;
    BB = A * Xd * Xc + B * Yd * Yc + C * Zd * Zc;
    CC = A * Xc * Xc + B * Yc * Yc + C * Zc * Zc - 1.0;
    D = BB * BB - AA * CC;
    if(D >= 0) {
        double t1, t2;
        t1 = (-BB - sqrt(D)) / AA + 1.0e-5;
        t2 = (-BB + sqrt(D)) / AA + 1.0e-5;
        if(t1 > 1.0e-3) {
            RRVec3 v1, v2;
            v1 = RRSetVec3(r.x + t1 * d.x, r.y + t1 * d.y, r.z + t1 * d.z);
            v2 = RRSetVec3(r.x + t2 * d.x, r.y + t2 * d.y, r.z + t2 * d.z);
            RRVec3 tosrc = RRSetVec3(-t1 * d.x, -t1 * d.y, -t1 * d.z);
            tosrc = RRNormalize(tosrc);
            RRRange range;
            range.s = RRDistance(r, v1);
            range.t = RRDistance(r, v2);
            RRVec3 norm;
            cross = v1;
            norm = rrCalcNorm(degree_z);
            arg->rrSetHit(true);
            arg->rrSetCross(v1);
            arg->rrSetNorm(norm);
            arg->rrSetToSrc(tosrc);
            arg->rrSetRange(range);
            return;
        }
        else {
            arg->rrSetHit(false);
            return;
        }
    }
    arg->rrSetHit(false);
    return;
}

RRVec3 RRQuadric::rrCalcNorm(double dz)
{
    RRVec3 N;
    double sgn;
    if(C > 0.0) {
        if(cross.z >= coord.z) sgn = 1.0;
        else sgn = -1.0;

        double SecX = cos(dz) * cross.x - sin(dz) * cross.y - coord.x;
        double SecY = sin(dz) * cross.x + cos(dz) * cross.y - coord.y;
        double Sec = sqrt(C * (1 - A * SecX * SecX - B * SecY * SecY));
        double Nx = (-A * SecX * cos(dz) - B * SecY * sin(dz)) / Sec;
        double Ny = (A * SecX * sin(dz) - B * SecY * cos(dz)) / Sec;

        N = RRSetVec3(Nx, Ny, sgn * 1.0);
        N = RRNormalize(N);
    }
    else if(B > 0.0) {
        if(cross.y >= coord.y) sgn = 1.0f;
        else sgn = -1.0f;

        double Sec;
        Sec = sqrtf(B * (1 - A * (cross.x - coord.x) * (cross.x - coord.x) - C * (cross.z - coord.z) * (cross.z - coord.z)));
        double Nx = A * (cross.x - coord.x) / Sec;
        double Nz = C * (cross.z - coord.z) / Sec;
        N = RRSetVec3(Nx, sgn * 1.0, Nz);
        N = RRNormalize(N);
    }
    else if(A > 0.0) {
        if(cross.z >= coord.z) sgn = 1.0f;
        else sgn = -1.0f;

        double Sec;
        Sec = sqrtf(A * (1 - B * (cross.y - coord.y) * (cross.y - coord.y) - C * (cross.z - coord.z) * (cross.z - coord.z)));
        double Ny = B * (cross.y - coord.y) / Sec;
        double Nz = C * (cross.z - coord.z) / Sec;
        N = RRSetVec3(sgn * 1.0, Ny, Nz);
        N = RRNormalize(N);
    }
    else
        N = RRSetVec3(0.0, 0.0, 0.0f);

    return N;
}
